/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This is an effort to test if the Occupancygrid can be used for SLAMing
*/ 

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include "CScanMatchInterface.h" ///< Scanmatch interface
#include "CAHistogramECorrelationScanMatch.h" ///< Scanmatch 
#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation

int readLaserOdometryData(FILE *f,ScanMatch::scan &meas, ScanMatch::pose &p){
		float p1,p2,p3;
		int i;
		char c;
		fscanf(f,"%f %f %f %f %f %f ", &p.x,&p.y,&p.a,&p1,&p2,&p3); ///< Pose
		for(i=0;i<361;i++){
				fscanf(f,"%f",&meas.r[i]);
				meas.a[i]=0.5*i*M_PI/180.0;
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<361){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
		meas.N = 361;
		return 0;

}


/// For reading the data from file
int readScanLine181(FILE *f,ScanMatch::scan &meas, ScanMatch::pose &p){
		float time_s;
		int i;
		char c;
		fscanf(f,"%f %f %f",&p.x,&p.y,&p.a); ///< Pose
	//fprintf(stderr,"1 ");
		for(i=0;i<181;i++){
				fscanf(f,"%f %f",&meas.r[i],&meas.a[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<181){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"3 \n");
		meas.N = 181;
		return 0;
}

void scan2absoluteXY( ScanMatch::scan &s, float *x, float *y, ScanMatch::pose &pose){
	int i;
	if(x==NULL || y==NULL) return;
	for(i=0;i<s.N;i++){
		x[i]=s.r[i]*cos(s.a[i]+pose.a)+pose.x;
		y[i]=s.r[i]*sin(s.a[i]+pose.a)+pose.y;
	}
}

float minMSD(float *x,float *y, float px,float py, int N, int &mini, float &dx2, float &dy2){
	float dist2=100000;
	float d2;
	int minInd = 0;
	float dx,dy;
	for(int i=0;i<N;i++){
		dx = (x[i]-px)*(x[i]-px);
		dy = (y[i]-py)*(y[i]-py);
		d2 = dx + dy;
		if(d2<dist2){ 
			dist2 = d2;
			dx2 = dx;
			dy2 = dy;
			minInd = i;
		}
	}
	mini = minInd;
	return dist2;
}

float computeMSE(ScanMatch::scan &s1,ScanMatch::scan &s2,ScanMatch::pose &p1,ScanMatch::pose &p2, 
							float &x_err ,float &y_err)
{
	float *x1 = (float *) malloc(s1.N*sizeof(float));  
	float *y1 = (float *) malloc(s1.N*sizeof(float));
	float *x2 = (float *) malloc(s2.N*sizeof(float));  
	float *y2 = (float *) malloc(s2.N*sizeof(float));

	long int ss1,us1,ss2,us2,sec,us,ms;
	ownTime_GetTimeOfDay(&ss1,&us1);

	scan2absoluteXY(s1,x1,y1,p1);
	scan2absoluteXY(s2,x2,y2,p2);
	
	int mini;
	float dist=0,d;
	float dx2=0,dy2=0,ddx,ddy;
	int num=0;
	for(int i=0;i<s1.N;i++){
		if(s1.r[i] <= 0 || s2.r[i]<=0) continue;
		d = minMSD(x2,y2,x1[i],y1[i],s2.N,mini,ddx,ddy);
		if(d<1.0){
			dist += d; 
			dx2 += ddx;
			dy2 += ddy;
			num++;
		}
	}	

	ownTime_GetTimeOfDay(&ss2,&us2);
	sec = ss2-ss1;us = us2-us1;ms = sec*1000+us/1000;
	float fms=(float)sec*1000.0+(float)us/1000.0;
	//fprintf(stderr,"It took %.4f ms to calculate\n",fms);
	
	//fprintf(stderr,"MSE= %.2f MSE(X)=%.2f MSE(Y)=%.2f", dist/num,dx2/num,dy2/num);
	//fprintf(stderr," ME= %.2f ME(X)=%.2f ME(Y)=%.2f\n", sqrt(dist/num),sqrt(dx2/num),sqrt(dy2/num));
	//x_err = sqrt(dx2/num);
	//y_err = sqrt(dy2/num);
	if(num!=0){
		x_err = sqrt(dx2/num);
		y_err = sqrt(dy2/num);
	}
	else{
		x_err = -666;
		y_err = -666;
	}
	free(x1);
	free(x2);
	free(y1);
	free(y2);
	
	if(num!=0){ 
		return dist/num;
	}
	return -666; 
}

int filterScan(ScanMatch::scan &m, Grid::TScan &g){
		int bad=0;
		float dist_th=1.0;
		float dr_plus,dr_minus;
		
		for(int i=1;i<m.N-1;i++){
				dr_plus = fabs(m.r[i]-m.r[i+1]);
				dr_minus = fabs(m.r[i]-m.r[i-1]);
				if(dr_plus>dist_th && dr_minus>dist_th) g.r[i] = -1.0; 
		}
		for(int i=0;i<m.N;i++){
				g.a[i] = m.a[i];
				if(g.r[i] == -1){ g.r[i]=0; bad++;} 
				else g.r[i] = m.r[i];
				if(g.r[i] > 35){g.r[i] = 0; bad++;}///Filter out long distances
		}
		return bad;
}

int main(void){
		TOccupancyGrid O(100.1,100.1,0.05,Grid::TCoord2D(0,0));
		TOccupancyGrid D(100.1,100.1,0.05,Grid::TCoord2D(0,0));
		TOccupancyGrid G(60.1,60.1,0.05,Grid::TCoord2D(0,0));
		grid_t empty=0.0;
		grid_t rigid=254;
		grid_t occu=128;
		grid_t unknown=64;
		int Nx,Ny;
		unsigned char *rgb;
		FILE *log = fopen("laser_update.txt","rt");
		
		///Scanmatch stuff
		CScanMatchInterface *matcher;  ///< This is for differential 
		CScanMatchInterface *gmatcher; ///< This is for global
		
		matcher = new CAHistogramECorrelationScanMatch(0.32, ///< Search area in meters 
																										0.5);///< Search angle in radians
		
		gmatcher = new CAHistogramECorrelationScanMatch(
																						.32,   // Search area in meters 
																						0.5);     // Search angle in radians
		
		ScanMatch::pose u_pos, pos,gpos,odo_cur,odo_ref,est,rpos,smpos;   ///< The estimated position
		//ScanMatch::scan ref,smref;   ///< The Reference scan 
		ScanMatch::scan meas,ref;  ///< The Reference scan 
		
		meas.allocate(361);
		ref.allocate(361);
		readLaserOdometryData(log,ref,odo_cur);
		odo_cur.set(0,0,0);
		matcher->setReferenceScan(ref,odo_cur); 
		
		///Initialize grid as empty
		//O.setGridToValue(unknown);
 		O.setGridToValue(0.5);
		D.setGridToValue(0.5);
		G.setGridToValue(0.5);
		O.getDim(Nx,Ny);
		rgb = (unsigned char *) malloc(3*Nx*Ny);
		///Initialize Image display
		addImageWindow(Nx, Ny);
		glutImageGUI_Start();
		/*
		G.load("labra.map");
		G.fillRGB(rgb,1.0/255.0);
		displayImage(0, rgb,Ny,Nx);
		usleep(1000*10000);
		*/
		
		Grid::TScan s,virt; ///< for occupancy grid
		s.alloc(361);
		virt.alloc(361);
		readLaserOdometryData(log,meas,odo_cur);
		for(int i=0;i<meas.N;i++){
				s.a[i] = meas.a[i];
				s.r[i] = meas.r[i];
		}
		odo_cur.set(0,0,0);
		//O.setScanFillSimple(odo_cur.x, odo_cur.y ,odo_cur.a, s,empty,occu);
		O.setScanFillProb(odo_cur.x, odo_cur.y ,odo_cur.a, s,0.01,0.99);
		O.fillRGB(rgb,1.0/255.0);
		displayImage(0, rgb,Ny,Nx);
		usleep(1000*2000);
		long int ss0,us0,ss1,us1,ss2,us2,sec,us,ms;
		float fms;
		ownTime_GetTimeOfDay(&ss0,&us0);
		matcher->getCurrentPose(smpos); ///< Get the estimate
		int cnt=0;
		odo_ref = odo_cur;
		//readLaserOdometryData(log,meas,odo_cur);
		while(!feof(log)){
				readLaserOdometryData(log,meas,odo_cur);
				
				ownTime_GetTimeOfDay(&ss1,&us1);
				
				est = est.calculateDifferentialSensorMovement(odo_ref,odo_cur, 0);
				
				
				
				//est.set(0.01 * ((float)(rand()%500)/500.0-0.5),0.01 * ((float)(rand()%500)/500.0-0.5),0.001 * ((float)(rand()%500)/500.0-0.5));
				odo_ref = odo_cur;
				pos = pos.integrateDifferential(est);
				
				//odo_cur.set(0,0,0);
				smpos = smpos.integrateDifferential(est);
				matcher->update(meas,smpos);    ///update
				matcher->getCurrentPose(smpos); ///< Get the estimate
				
				matcher->setReferenceScan(meas,smpos); 
				//gpos = gpos.integrateDifferential(smpos);	
				gpos = smpos;
				
				
				//fprintf(stderr,"1:(%.2f,%.2f,%.2f) ",pos.x,pos.y,pos.a);
				ownTime_GetTimeOfDay(&ss2,&us2);
				sec = ss2-ss1;
				us = us2-us1;
				ms = sec*1000+us/1000;
				fms=(float)sec*1000.0+(float)us/1000.0;
	      fprintf(stderr,"1: (%.4f) ",fms);
				ownTime_GetTimeOfDay(&ss1,&us1);
				//O.getScan(pos.x,pos.y, (pos.a-M_PI/2.0), 181, M_PI/180.0, occu, virt); ///<getscan
				O.getScan(pos.x,pos.y, (pos.a), 361, M_PI/360.0, (grid_t) 0.7, virt); ///<getscan
				int badval=0;
				
				
				
				for(int i=0;i<meas.N;i++){
						ref.a[i]=virt.a[i];
						ref.r[i]=virt.r[i]; 
						if (virt.r[i]<0){ref.r[i]=0;badval++;}
				}
				//if(badval >= 70){ fprintf(stderr,"Badval=%d\n",badval);}
				fprintf(stderr,"Badval=%d ",badval);
				ownTime_GetTimeOfDay(&ss2,&us2);
				sec = ss2-ss1;
				us = us2-us1;
				ms = sec*1000+us/1000;
				fms=(float)sec*1000.0+(float)us/1000.0;
				fprintf(stderr,"2:Get (%.4f) ",fms);
				
				//fprintf(stderr,"2:(%.2f,%.2f,%.2f) ",pos.x,pos.y,pos.a);
				ownTime_GetTimeOfDay(&ss1,&us1);
				
				if(badval < 120){
						gmatcher->setCurrentPose(pos);
						gmatcher->setReferenceScan(ref,pos); 
						gmatcher->update(meas,pos);    ///update
						gmatcher->getCurrentPose(pos); ///< Get the estimate
				}
				//fprintf(stderr,"3:(%.2f,%.2f,%.2f) \n",pos.x,pos.y,pos.a);
				ownTime_GetTimeOfDay(&ss2,&us2);
				sec = ss2-ss1;us = us2-us1; ms = sec*1000+us/1000;
				fms=(float)sec*1000.0+(float)us/1000.0;
				fprintf(stderr,"3: Gpos (%.4f) ",fms);
				
				O.getScan(pos.x,pos.y, (pos.a), 361, M_PI/360.0, (grid_t) 0.7, virt); ///<getscan
				
				for(int i=0;i<meas.N;i++){
					ref.a[i]=virt.a[i];
					ref.r[i]=virt.r[i]; 
					if(virt.r[i]<0){
							virt.r[i]=0; //badval++;
					}
				}
				float de,xe,ye;
				de = computeMSE(meas,ref,pos,pos,xe ,ye);
												
				//matcher->setReferenceScan(meas,pos); 
				//matcher->setCurrentPose(pos);
				
				/*
				for(int i=0;i<meas.N;i++){
						s.a[i] = meas.a[i];
						s.r[i] = meas.r[i];
						if(s.r[i] > 35) s.r[i] = 0;
				}*/
				filterScan(meas,s);
				
				ownTime_GetTimeOfDay(&ss1,&us1);
				//if( sqrt((pos.x-u_pos.x)*(pos.x-u_pos.x) + (pos.x-u_pos.y)*(pos.y-u_pos.y))>2.0 ||
				//				fabs(pos.a-u_pos.a)>M_PI/4.0			){
				//O.setScanFillSimple(pos.x, pos.y ,pos.a, s,empty,occu,unknown);
				float prob;
				if(de>0) prob = 0.1+0.4*exp(-de/0.05);
				else prob = 0.2;
				//O.setScanFillProb(odo_cur.x, odo_cur.y ,odo_cur.a, s,0.5-prob,0.5+prob);
				if(badval<120) O.setScanFillProb(pos.x, pos.y ,pos.a, s,0.1,0.9);
				else O.setScanFillProb(pos.x, pos.y ,pos.a, s,0.1,0.9);
				
				D.setScanFillProb(pos.x, pos.y ,pos.a, s,0.3,0.9);
				//D.setScanFillProb(gpos.x, gpos.y ,gpos.a, s,0.3,0.9);
				
				//D.setScanFillProb(odo_cur.x, odo_cur.y ,odo_cur.a, s,0.4,0.65);
				D.setValue(Grid::TCoord2D(gpos.x,gpos.y), 1.0);
				//D.setValue(Grid::TCoord2D(odo_cur.x,odo_cur.y), .8);
				ownTime_GetTimeOfDay(&ss2,&us2);
				sec = ss2-ss1;us = us2-us1; ms = sec*1000+us/1000;
				fms=(float)sec*1000.0+(float)us/1000.0;
				fprintf(stderr,"4:Set (%.4f), err(%.2f,%.2f,%.2f) P=%.3f\n",fms,xe,ye,sqrt(de),0.5+0.5*exp(-de/0.05));
				
				u_pos = pos;
				//}
				
				//O.setScanFillSimple(pos.x+5, pos.y+5 ,0, virt,empty,occu);
				if(cnt%5==0) {
						D.fillRGB(rgb,1.0/255.0);
						displayImage(0, rgb,Ny,Nx);
				}
				//usleep(1000*500);
				cnt++;
		}
		/*
		Grid::TCoord2D coord;
		grid_t v;
		float x,y,l,a;
		fprintf(stderr,"\nSetting the map to Global map\n");
		for(int j=0;j<Ny;j++){
		  for(int i=0;i<Nx;i++){
			 O.getValue(Grid::TPixel(i,j),v);
			 coord = O.toCoord(Grid::TPixel(i,j));
			 l = sqrt(coord.x*coord.x + coord.y*coord.y);
			 a = atan2(coord.y,coord.x);
			 x=l*cos(a-20.0*M_PI/180.0) + 2.0;
			 y=l*sin(a-20.0*M_PI/180.0) - 3.0;
			 coord.x = x; coord.y=y;
			 G.setValue(coord,v);
			}
		}
		fprintf(stderr,"Done!!\n");
		G.fillRGB(rgb,1.0/255.0);
		displayImage(0, rgb,Ny,Nx);
		usleep(1000*5000);
		
		
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss0;us = us2-us0; ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		fprintf(stderr,"TOTAL::(%.4f) = (%.4f)/scan \n",fms,fms/cnt);
		G.save("labra.map");
		*/
		return 0;
}